#include "colormarker.h"
#include <QDebug>

ColorMarker::ColorMarker( double x, double y, int h )
    :Point2f( x, y )
{
    hue = h;
}


// not implemented
double ColorMarker::compareEMD( ColorMarker that )
{
    SparseMat thist = that.hist;
    Mat sig1( HBINS * SBINS, 3, CV_32FC1, 0 ), sig2( HBINS * SBINS, 3, CV_32FC1, 0 );

    for( int h = 0; h < HBINS; h ++ )
    {
        for( int s = 0; s < SBINS; s ++ )
        {
            //float bin_val = this->hist.at<double>( h, s );

        }
    }
    return 0;
}

ColorMarkerDetector::ColorMarkerDetector(int width, int height)
{
    w = width;
    h = height;
    CvSize size = cvSize( w, h );
    cvImg32   = cvCreateImage( size, 32, 1 );
    xc_points = new CvPoint2D32f[MAX_XCORNERS];
    xc_finder = new CXCornerFinder;
    xc_finder->SetMaximumSValue( -100.f );
    xc_finder->SetSigma( 2.0f );
}


ColorMarkerDetector::~ColorMarkerDetector()
{
    cvReleaseImage( &cvImg32 );
    delete xc_points;
    delete xc_finder;
}

void ColorMarkerDetector::detect( Mat &input, vector<ColorMarker> &list, int outer )
{
    list.clear();
    if( input.type() != CV_8UC3 )
    {
        cout << "Wrong type of input image. Need CV_8UC3";
        return;
    }

    // first, find all possible corners
    Mat inputgray, inputgray32;
    IplImage temp;
    cvtColor( input, inputgray, CV_BGR2GRAY );
    inputgray.convertTo( inputgray32, CV_32F, 1.f );
    temp = inputgray32;
    cvCopy( &temp, cvImg32 );
    xc_finder->SetImage( cvImg32 );
    int pnts_found = xc_finder->FindXCorners( xc_points, MAX_XCORNERS, false );

    if( pnts_found == 0 ) return;

    // check the color around every markers
    Mat inputhsv, mask, wino, masko;
    cvtColor( input, inputhsv, CV_BGR2HSV );

    int hbins = HBINS, sbins = SBINS;
    int histSize[] = { hbins, sbins };
    float hranges[] = { 0, 360 };
    float sranges[] = { 0, 256 };
    const float* ranges[]= { hranges, sranges };
    int channels[] = { 0, 1 };
    MatND hist;
    inRange( inputhsv, Scalar( 0, 100, 100 ), Scalar( 360, 256, 256 ), mask );
    //imshow( "mask", mask );

    for( int i = 0; i < pnts_found; i ++ )
    {
        Rect rect = Rect( xc_points[i].x - outer,
                          xc_points[i].y - outer,
                          outer * 2, outer * 2 ) ;
        if( xc_points[i].x - outer < 0 || xc_points[i].y - outer < 0
                || xc_points[i].x + outer > w || xc_points[i].y + outer > h )
        {
            continue;
        }
        inputhsv( rect ).copyTo( wino );
        masko = mask( rect );
        circle( input, Point( xc_points[i] ), 2, Scalar( 0, 255, 0 ), 2 );
        rectangle( input, rect, Scalar( 255, 255, 0 ), 1  );
        calcHist( &wino, 1, channels, masko, hist, 2, histSize, ranges );
        double maxVal=0;
        Point maxID;
        minMaxLoc( hist, 0, &maxVal, 0, &maxID  );
        if( maxVal > 5 )
        {
            ColorMarker newmarker = ColorMarker( xc_points[i].x, xc_points[i].y, maxID.y );
            newmarker.hist = hist;
            list.push_back( newmarker );
//            QString str = "H" + QString::number( maxID.y * 24 );
//            putText( input, str.toStdString(), Point( xc_points[i] ), FONT_HERSHEY_SIMPLEX, 0.5,
//                     Scalar( 0, 0, 0 ), 2 );
//            putText( input, str.toStdString(), Point( xc_points[i] ), FONT_HERSHEY_SIMPLEX, 0.5,
//                     Scalar( 255, 255, 255 ), 1 );
        }
        //imshow( "input", input );
    }
}


Scalar ColorMarkerDetector::RGB2HSV( Scalar c )
{
    int R = c[0], G = c[1], B = c[2];
    return RGB2HSV( R, G, B );
}

Scalar ColorMarkerDetector::RGB2HSV(int R, int G, int B) {
    double r = (double) R / 255.0;
    double g = (double) G / 255.0;
    double b = (double) B / 255.0;

    double maxVal = (double) max(r , max(g,b) );
    double minVal = (double) min(r , min(g,b) );

    double h,s,v;

    v = maxVal;
    s = (maxVal != 0.0) ? ((maxVal - minVal) / maxVal) : 0.0;

    if (maxVal == 0)
    {
        return Scalar( -1, 0, 0 );
    }

    if(s == 0.0)
    {
        h = -1;
    }
    else
    {
        double delta = maxVal - minVal;
        if (r == maxVal)
        {
            h = (g - b) / delta;
        }
        else if (g == maxVal)
        {
            h = 2.0 + (b - r) / delta;
        }
        else if (b == maxVal)
        {
            h = 4.0 + (r - g) / delta;
        }
        h *= 60.0;

        if( h < 0.0 )
        {
            h += 360.0;
        }
    }
    return Scalar( h, s * 255, v * 255 );
}
